// libc
#include <stdint.h>
#include <math.h>
// arduino lib
#include <SPI.h>
// external lib
#include <XBee.h>

#define COLUMN_MAX  16
#define ROW_MAX     8

// Shift register latch
#define PIN_LATCH   8

// SPI pins
#define PIN_SPI_SS      10  // slave select
#define PIN_SPI_MOSI    11  // output
#define PIN_SPI_MISO    12  // input
#define PIN_SPI_SCK     13  // clock

// Led number
#define LED_1       0x1
#define LED_2       0x2
#define LED_3       0x4
#define LED_4       0x8
#define LED_5       0x10
#define LED_6       0x20
#define LED_7       0x40
#define LED_8       0x80

#define STATE_BLUE  0x1
#define STATE_GREEN 0x100
#define STATE_RED   0x10000

struct led {
    uint32_t state;
    uint8_t intensity;
};

struct led matrix[COLUMN_MAX][ROW_MAX];

XBee xbee = XBee();

static void spi_init() {
    int8_t clr;

    // Define all pinout for SPI
    pinMode(PIN_LATCH, OUTPUT);
    pinMode(PIN_SPI_SCK, OUTPUT);
    pinMode(PIN_SPI_MOSI, OUTPUT);
    pinMode(PIN_SPI_SS, OUTPUT);
    pinMode(PIN_SPI_MISO, INPUT);

    digitalWrite(PIN_SPI_SS, HIGH);

    // Init SPI module
    SPCR = (1<<SPE) | (1<<MSTR) | (1<<SPR1) | 1;

    // clear garbage by reading
    clr=SPSR;
    clr=SPDR;

    delay(10);
}

static void led_init() {
    for (uint8_t col_count = 0; col_count < COLUMN_MAX; col_count++) {
        for (uint8_t row_count = 0; row_count < ROW_MAX; row_count++) {
            // Full intensity
            matrix[col_count][row_count].intensity = 1;
            // All off
            matrix[col_count][row_count].state = 0;
        }
    }
}

static inline uint8_t shiftout(uint8_t byte) {
    SPDR = byte;

    // Wait for the end of the transmission
    while (!(SPSR & (1<<SPIF)));

    return SPDR;    // SPIF will be cleared
}


void setup() {
    // Serial init a 9600 baud
    Serial.begin(9600);

    // XBee init
    xbee.begin(9600);

    // Shift register initiation
    spi_init();

    // Init all led to OFF
    led_init();
}

// Current column while iterating
uint8_t column_current = 0;
// Column data
uint16_t column_data = 0;
// Cycle
uint32_t cycle = 0;
// Xbee response
Rx16Response rx16 = Rx16Response();

void loop() {
    //uint8_t option = 0;
    uint8_t x_pos = 0;
    uint8_t y_pos = 0;
    uint8_t color = 0;

    cycle++;

    // Read a XBee packet
    xbee.readPacket(1);

    if (xbee.getResponse().isAvailable()) {
        xbee.getResponse().getRx16Response(rx16);
        //option = rx16.getOption();

        x_pos = rx16.getData(0);
        y_pos = rx16.getData(1);
        color = rx16.getData(2);

        if (x_pos >= 8) {
            y_pos += 8;
            x_pos %= 8;
        }

        switch (color) {
            case 1:
                matrix[y_pos][x_pos].state = ((uint32_t) STATE_BLUE) << x_pos;
                break;
            case 2:
                matrix[y_pos][x_pos].state = ((uint32_t) STATE_GREEN) << x_pos;
                break;
            case 3:
                matrix[y_pos][x_pos].state = ((uint32_t) (STATE_BLUE | STATE_GREEN)) << x_pos;
                break;
            case 4:
                matrix[y_pos][x_pos].state = ((uint32_t) STATE_RED) << x_pos;
                break;
            case 5:
                matrix[y_pos][x_pos].state = ((uint32_t) (STATE_RED | STATE_BLUE)) << x_pos;
                break;
            case 6:
                matrix[y_pos][x_pos].state = ((uint32_t) (STATE_RED | STATE_GREEN)) << x_pos;
                break;
            default:
            case 7:
                matrix[y_pos][x_pos].state = ((uint32_t)(STATE_RED | STATE_GREEN | STATE_BLUE)) << x_pos;
                break;
        }

        matrix[y_pos][x_pos].intensity = 1;
    }


    // Display the matrix
    for (column_current = 0; column_current < COLUMN_MAX; column_current++) {
        column_data = 1 << column_current;

        // ROW
        uint32_t data = 0;

        for (uint8_t  row_count = 0; row_count < ROW_MAX; row_count++) {
            if (cycle % matrix[column_current][row_count].intensity == 0) {
                data |= matrix[column_current][row_count].state;
            }

            if (cycle % 125 == 0) {
                ++matrix[column_current][row_count].intensity;

                if (matrix[column_current][row_count].intensity == 6) {
                    matrix[column_current][row_count].intensity = 1;
                    matrix[column_current][row_count].state = 0;
                }
            }
        }

        if (data == 0) {
            continue;
        }

        // DISPLAY OFF
        digitalWrite(PIN_LATCH, LOW);

        // COLUMN
        shiftout(column_data >> 8);
        shiftout(column_data);

        // Row data is inverted
        shiftout(255- (data & 0xFF));
        shiftout(255- ((data >> 8) & 0xFF));
        shiftout(255- ((data >> 16) & 0xFF));

        // DISPLAY ON
        digitalWrite(PIN_LATCH, HIGH);
    }
}

